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Abstract: Model predictive control (MFC) anticipates future events to take appropriate control 
actions. Nonlinear MFC (NMFC) describes systems with nonlinear models and/or constraints. 
Continuation MFC, suggested by T. Ohtsuka in 2004, uses Krylov-Newton iterations. Contin¬ 
uation MFC is suitable for nonlinear problems and has been recently adopted for minimum 
time problems. We extend the continuation MFC approach to a case where the state is 
implicitly constrained to a smooth manifold. We propose an algorithm for on-line controller 
implementation and demonstrate its numerical effectiveness for a test problem on a hemisphere. 
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1. INTRODUCTION 

Model predictive control (MFC) is exposed, e.g., in Ca¬ 
macho et al. (2004); Griine et al. (2011). MFC is efficient 
in industrial applications; see Qin et al. (2003). Numerical 
aspects of MFC are discussed in Diehl et al. (2009). Our 
contribution to numerical methods for MFC is further 
development of the approach by Ohtsuka (2004) and effi¬ 
cient use of the Newton-Krylov approximation for numer¬ 
ical solving the nonlinear MFC problems. Knyazev et al. 
(2015a) solves a minimum-time problem and investigates 
preconditioning for the Newton-Krylov method. 

The MFC method owes its success to efficient treatment 
of constraints on control and state variables. The goal of 
the present note is to draw attention to the important 
fact that, in addition to the efficient treatment of explicit 
constraints, MFC can easily incorporate the structure¬ 
preserving geometric integration of ordinary differential 
equations modeling the system. 

The idea of structure-preserving numerical methods for 
differential equations has been employed in numerical 
analysis since 1950s, when the first computers began to be 
used in engineering computations. However, its systematic 
study under the name of geometric integration of differen¬ 
tial equations has been accomplished within the last 40 
years. An accessible introduction to this active research 
domain is found in Hairer (2011). A more advanced source 
is Hairer et al. (2006). 

Variational formulations of many models described by dif¬ 
ferential equations lead to the Hamiltonian system dynam¬ 
ics; see Leimkuhler et al. (2004). Such dynamic systems 
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conserve the energy and possess a special geometric struc¬ 
ture called the symplectic structure. Much effort has been 
spent by numerical analysts to develop the called symplec¬ 
tic numerical methods, which are especially efficient for 
long-time integration, because they produce qualitatively 
correct computed solutions. The symplectic methods are 
superior even in the short-time simulations. Leimkuhler 
et al. (2004) presents the state-of-the-art symplectic nu¬ 
merical algorithms and supporting theory. 

Description of the system dynamics in the form of ordinary 
differential equations appears twice in MFC: first, within 
the finite-horizon prediction problem and, secondly, when 
advancing the current state with the input control com¬ 
puted by the finite-horizon prediction. The latter can be 
omitted if the states of the system are measured directly 
by the controller sensors. Furthermore, the application of 
the geometric structure-preserving integration algorithms 
from Hairer et al. (2006) or other sources for advancing 
the current state is straightforward. 

The use of the geometric integration during the finite- 
horizon prediction is less obvious. The main difference 
between the method from Knyazev et al. (2015a) and 
the variant of the present paper is in that the latter 
applies a structure-preserving geometric integration inside 
the forward recursion. 

The rest of the note is organized as follows. Section 2 
presents a framework of the finite-horizon prediction prob¬ 
lem and expresses its solution in the form of a nonlinear 
algebraic equation. Geometric integration is incorporated 
during the elimination of state variables from the KKT 
conditions. Section 3 discusses how classical integration 
methods can be used for integration on manifolds. The 
content of Section 3 is mainly adopted from Hairer (2001) 
and discusses the local coordinates approach and projec- 



tion methods. Section 4 describes a simple test example 
and necessary formulas for computer implementation. Sec¬ 
tion 5 shows numerical results and plots. 


2. FINITE-HORIZON PREDICTION 

Our model finite-horizon control problem, see Knyazev 
et al. (2015a), along a fictitious time r € [t,t-\-T] consists 
in choosing the control u{t) and parameter vector p, which 
minimize the performance index J as follows: 

min J, 

u,p 

where 

t+T 

J = (j){x{t + T),p) + J L{T,x{T),u{T),p)dT 

t 

subject to the equation for the state dynamics 

(IT 

— = f{T,x{T),u{T),p), (1) 

and the equality constraints for the state x and the 
control u 

C{t,x{t),u{t),p) =0, ( 2 ) 

i;{x{t + T),p) =0. (3) 

The initial value condition x{T)\r=t for (1) is the state 

vector x{t) of the dynamic system. The control vector 

u = u(T)\r=t is used afterwards as an input to control 
the system at time t. The components of the vector p{t) 
are parameters of the dynamic system. The horizon time 
length T may depend on t. 

The continuous formulation of the hnite-horizon prediction 
problem stated above is discretized on a time grid r^, 
i = 0,1,... ,N, through the horizon [t, t + T] partitioned 
into N time steps of size At^ = Ti+i — Ti, and the time- 
continuous vector functions x{t) and u{t) are replaced by 
their indexed values Xi and Ui at the grid points Ti. The 
integral of the performance cost J over the horizon is ap¬ 
proximated by the rectangular quadrature rule. Equation 
(1) is approximated by a one-step integration formula, for 
example, a Runge-Kutta method; cf. Hairer (2001). The 
discretized optimal control problem is as follows: 

min <j){xN,p) + 

Ui ,p 



N-1 

C(X,U) = (I){xn,p) + ^ L{TijXi,Ui,p)ATi 
+ Ao[x(t) -Xo] 

A^-1 

+ ^ - Xi+i + ^i{ri,Xi,Ui,p)Ari] 

i^O 
N-1 

+ ^ fi[C{ri,Xi,Ui,p)Ari + ly'^ipixNjP), 
i^O 

where X = [xi , i = 0,1,... ,N, and U = [ui pt v p]'^, 
i = 0,1,..., N—1. Here, A is the costate vector and p is the 
Lagrange multiplier vector associated with constraint (5). 
The terminal constraint (6) is relaxed by the aid of the 
Lagrange multiplier v. 

The Hamiltonian function is denoted by 

H[t, X, A, u, p,p) = L{t, X, u,p) 

+ f{t, X, u,p) -\- p^C{t, X, u,p). 

The necessary optimality conditions are the KKT sta- 
tionarity conditions: L\. = 0, = 0, i = 0,1,..., A, 

N, = 0, = 0, z = 0,1,..., A - 1, £,, = 0, Ap, = 0. 

Since d^i{Ti,Xi,Ui,p)/dxi is not always available, we sug¬ 
gest to use df(Ti,Xi,Ui,p)/dxi instead. This modification 
is applied in the backward recursion used below. 

The KKT conditions are reformulated in terms of a map¬ 
ping F\U,x,t], where the vector U combines the control 
input u, the Lagrange multiplier p, the Lagrange multiplier 
v, and the parameter p, all in one vector: 

U{t) = 

The vector argument x in F[U,x,t] denotes the current 
measured or estimated state vector, which serves as the 
initial vector xq in the following procedure. 

(1) Starting from the current measured or estimated state 
Xq, compute Xi, i = 0,1..., A — 1, by the forward 
recursion 

Xt+i = Xt + ^i(Ti,Xi,Ui,p)ATi. 


subject to 

Xi+i = x^ + ^i{Ti,Xi,Ui,p)ATi, z = 0, 1, ..., A - 1,(4) 
C{Ti,Xi,Ui,p) = 0, z = 0,1,..., A - 1, (5) 

^{xN,p)=0. ( 6 ) 

The function <I>i is implicitly determined by the structure¬ 
preserving method used for numerical integration of (1). 
We note that ^i{n,Xt,Ut,p) = f{Ti,Xi,Ui,p) + 0(At), 
where At = maxi Ar^. 

The necessary optimality conditions for the discretized 
finite horizon problem are obtained by means of the 
discrete Lagrangian function 


Then starting from 

dd)^ dilT 

Xn = -^{xN,p) + -^{xN,p)v 

compute the costates Xi, i = A —1,...,0, by the 
backward recursion 

Ai — Aj-i-i “h (t^ , Xi , Aj_^i, Ui , p-i , p^Ati . 

(2) Calculate F\U,x,t], using just obtained Xi and Xi, as 




The coordinate change x = a(z) transforms the differential 
equation x = f{x) into 

a'{z)z = f{aiz)). (8) 

This is an over-determined system of differential equations 
because the dimension of z is less than n = dimx. 
However, f(x) is tangent to A4 by assumption, therefore, 
(8) is equivalent to a system 

z = /3(z), z(n) = Zi. (9) 

If the transfer from (8) to (9) is easy for implementation, 
then the method of local coordinates is feasible. 

The principal idea of the method of local coordinates is 
to perform one step of a numerical method applied to (9) 
and to map the result via the transformation a back to the 
manifold. One step Xi i—>■ x^+iof the resulting algorithm is 
implemented as follows. 

Algorithm 1 (Local coordinates approach) 

• choose a local parametrization a and compute Zi from 

Xi = a{zi)] 

• compute Zi+i = Zi + $i(zi)Ari, the result of the one- 
step method applied to (9); 

• dehne the numerical solution by Xi^i = a(zi+i). 


The equation with respect to the unknown vector U (t) 

F[U{t),x{t),t]=0 (7) 

gives the required necessary optimality conditions that are 
solved on the controller in real time. All details about 
solving (7) by the Newton-Krylov method are found in 
Knyazev et al. (2015a). We only recall that our method 
uses GMRES iterations for solving linear systems with 
the Jacobian matrix Fjj. To accelerate the convergence 
of GMRES iterations, the matrix Fjj is computed exactly 
at some time instances and then used as a preconditioner 
between these time instances. 

3. GEOMETRIC INTEGRATION 

This section gives a short explanation of geometric inte¬ 
gration used in our illustrative example. We consider the 
ordinary differential equations (1), which describe the sys¬ 
tem dynamics. Suppose that a property L{x(t)) = const 
is fulfilled on each solution x{t) of (1), where the constant 
const depends on the solution. If the numerical method 
Xi+i = XiF ^i{Ti,Xi,Ui,p)ATi satisfies the same property 
L(xi{T)) = const, we refer to it as a structure preserving 
method. A notorious example is given by the spheres 
L(x{t)) = ||a ;||2 = const. 

The article by Hairer (2001) “illustrates how classical 
integration methods for differential equations on manifolds 
can be modihed in order to preserve certain geometric 
properties of the exact flow. Projection methods as well 
as integrators based on local coordinates are considered.” 

We adopt the simplest method from Hairer (2001), which 
is called the method of local coordinates there. When it is 
feasible, it is the most accurate of all structure preserving 
methods. Let a manifold Ai contain the solution of i = 
f{x), which we want to compute. Assume that a:U —> i?” 
is a local parametrization of AA near the state Xi = a(zi). 


3.1 Symmetric integration methods 

Mechanical systems often obey the Hamiltonian structure, 
and numerical methods for such system must be symmetric 
or time-reversible in order to preserve geometric proper¬ 
ties of the Hamiltonian systems. Assume that a one-step 
numerical method applied to a system x = f(t, x) over the 
interval [U, U+i] produces the state Xi+i from the state Xi. 
The time-reversibility of the numerical method means that 
its application to the time-reversed system —x = f{—t,x) 
over the interval [—ti+i,—ti\ produces Xi from Xi+i. This 
is a case for the trapezoidal rule, but not for the explicit 
Euler method. 

Hairer (2001) shows that symmetric methods perform 
qualitatively better for integration over long time intervals. 
But most of the commonly used techniques for solving 
differential equations on manifolds destroy the symmetry 
of underlying method. To restore the symmetry, additional 
modihcations of numerical methods are necessary. 

We illustrate restoration of the symmetry for another 
standard technique of geometric integration on manifolds 
called the projection methods. For an ordinary differential 
equation y = f{y), the one step integration i—>■ y„+i 
proceeds as follows. 

Algorithm 2 (Standard projection method) 

• compute ijn+i = ^hiVn) by any numerical integrator 
<i>/i applied to y = f{y), e.g. by a Runge-Kutta 
method; 

• project y„+i orthogonally onto the manifold Ad to 
obtain yn+i € Ad. 

The standard projection method is illustrated in Figure 1. 
The projection destroys the symmetry of <I>?i if it is 
available. A symmetry-restoration algorithm is developed 
in Hairer (2000). The idea is to perturb the vector 




Fig. 1. The standard projection (Hairer, 2001, Fig. 3.1) 

before applying a symmetric one-step method such that 
the final projection is of the same size as the perturbation. 

Algorithm 3 (Symmetric projection method) 

• yn = yn + where g(y„) = 0; 

• y„+i = ^hiVn) (symmetric one-step method for 
equationj) = f{y)); 

• yn+i = yn +1 + G"’"{yn+i)y with vector /i such that 
givn+i) = 0. 

Here, G{y) = g'{y) denotes the Jacobian of g{y), if the 
manifold is given by the condition M = {y\g{y) = 0}. It is 
important to choose the same vector y in the perturbation 
an in the projection. 



4. TEST PROBLEM 


We consider the minimum-time motion over the unit upper 
hemisphere from a state {xo,yo,zo) to a state {xf,yf,Zf) 
with inequality constraints. 


The system dynamics is governed by the system of differ¬ 
ential equations 


' X ~ 


z cos u 

y 

= 

zsinu 

_ z _ 


_ —xcosu — ysmu_ 


( 10 ) 


The control variable u is subject to an inequality con¬ 
straint. Namely, the control u always stays within the 
band c„ — r„ < m < c„ -I- r„. Following Ohtsuka (2004) 
we introduce a slack variable Us and replace the inequality 
constraint by the equality constraint 

C{u, Ud) = {u- Cuf + ul-rl = 0. 


In order to guarantee that the state passes through the 
point {xf,yf,Zf) at time t = tf, we impose three terminal 
constraints of the form 


ip{x,y,z,p) 


X-Xf 

y-yj 

-Z-Zf . 


= 0 . 


The objective is to minimize the cost function 
tf 

J = (j){p)+ J L{x,y,z, u, Us, p)dt', 

io 


where 

Hp) = P = tf - to, L{x, y, z, U, Us,p) = -WsUs- 


The term (j){p) is responsible for the shortest time to 
destination, and the function L serves for stabilization of 
the slack variable Us- 

System (10) possesses the first integral 

x"^ + y'^ + z"^ = const. 

Our initial condition lies on the unit sphere XQ+yQ+ZQ = 1. 
Therefore, the manifold A4 is the unit sphere with the 
center at the origin. To simplify the implementation, we 
choose all parameters such that the trajectories lie on the 
upper hemisphere 2 : > 0. The natural local coordinates 
in this case are x and y, and the local parametrization is 
given by a{x,y) = [x,y, \/l — x"^ — The system (9) 
for (10) will be 


d 

X 


a/I — x^ — cos u 

dt 

.y. 


■\/l — x^ — sin u 


For this particular example and choice of local coordinates 
the structure preserving geometric integration solver con¬ 
sists of the explicit Euler method for the components x and 
y and the formula z = ■\/l — for the component z. 

For convenience, we change the time variable t within the 
horizon by the new time t = {t — to)/{tf — to), which runs 
over the interval [0,1]. 

The corresponding discretized finite-horizon problem on a 
uniform grid Ti in the local coordinates x, y comprises the 
following data structures and computations: 


• Ti = iAr, where i = 0,1,..., N, and At = 1/iV; 

Xi 


• the participating variables are the state 

u. 


yi 


the 


costate 


Ai,i 


, the control 


u 


SI 


, the Lagrange 


Vl 

V2 


multipliers pi and 

• the state is governed by the model equation 

Xi+i = Xi + At xf - yf cos , 

yi+i =yi + At (^p\/l - x^ - y"^ sinwi^ , 
where f = 0,1,..., A — 1; 

• the costate is computed by the backward recursion 
(Ai^at = ifi, A 2 ,Ar = 122) 

• (cosMiAi.*+i + sm'UiA2.i-i-i), 

• (cosMiAi.A+i + sm'UiA2.i-i-i), 
where i = N — 1, N — 2,... ,0; 
the nonlinear equation F{U,xo,to) = 0, where 


U = [mo, • ■ • , UN-1,Us, 0, ■ ■ ■ , Us^N-1, 

yo,---,PN-l,l^l,’y2,p], 
has the following rows from the top to bottom: 

I Atp [^1 - xf - yf (-sinUjApi-i-i J-cos UiA2.i-i-i) 


I 


-|- 2 (lij c^) ^ 2 ] — 0 



{ Arp [2piUsi - Ws] = 0 


0.65 


{ Arp [(u* - Cuf + ul, -rl]=Q 

J Xn ~ 3 ^/ = 0 

I VN-yf = 0 


N-1 - 

Ar{^ Y 1 - a:? - yf {cosUiXi^i+i + sinwiA 2 ,i+i) 
+ Pi[{ui - Cuf + uli - rl] - WsUsi} + 1 = 0. 


5. NUMERICAL RESULTS 

In our numerical experiments, we use the method of the 
local coordinates for geometric integration on the unit 
upper hemisphere. 

The number of grid points on the horizon is = 20, 
the time step of the dynamic system is At = 0.00625, 
the end points of the computed trajectory are (xo,?/o) = 
(—0.5,0.5) and {xf,yf) = (0.5,0). The constants of the 
inequality constraint for the control are Cu = 0.5 and 
r„ = 0.1. Other parameters are h = 10“® and Ws = 0.005. 

We use the GMRES method without restarts. The number 
of GMRES iterations does not exceed fcmax = 20, and the 
absolute tolerance of the GMRES iterations is 10“^. 

Preconditioning of GMRES accelerates its convergence 
as demonstrated on Figures 6 and 7. We used a simple 
preconditioning strategy as follows. The exact Jacobian 
Fij is computed periodically at time instances with the 
period 0.2 seconds. Then the LU factorization of the 
Jacobian is used as the preconditioner until the next time 
when it is recalculated. 

The value of U at to is approximated by the Matlab 
function f solve with a special initial guess. 



Fig. 4. The constrained control 



Fig. 5. 2D projection of the trajectory 



Fig. 3. 3D plot of the trajectory on the unit hemisphere. 
Time to destination equals 1.2332. 


6. GONGLUSION 


Model predictive control is efficient in dealing with the 
constraints on the control and state variables. When the 
state space of a system lies on a manifold, it may be 
profitable to use numerical methods which inherit this 
property. For example, numerical algorithms preserving 
the symplectic structure of Hamiltonian dynamical sys¬ 
tems are much superior in accuracy compared to the 
conventional algorithms. In this note, we show how to 
incorporate simple modifications of classical integration 
methods into our numerical approach to MPG obtaining 
an efficient structure-preserving NMPG method. Other 
structure-preserving algorithms can be used similarly. 
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Fig. 7. Number of GMRES iterations with preconditioning 
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Fig. 8. Norm of the residual ||F|j 2 

















































